#include "control_task.h"
#include <string.h>
#include <math.h>

Chariot chariot;
extern uint8_t usart3_RX_buf[USART_RX_BUF_LENGTH];

float Init_Angle = 0 ;

float Set_Yaw_Angle_1 = 0;
float Set_Yaw_Angle_2 = 0;

float Dart_parameter[4][5] = {
    // yaw_angle , high_speed ,low_speed ,push_length
    {-1.8, 8125, 7825, push_length_1,29.71},//劳5
    {-1.4, 7575, 7275, push_length_2,29.71},//劳6
    {-1.8, 7650, 7350, push_length_3,29.71},//劳7
    {-1.82, 7530, 7230, push_length_4,29.71},//劳8
};
uint8_t ID = 0;
uint8_t Start_Flag = 0;
uint8_t Last_Status = 0;

void Dart_FSM_TIM_Status_PeriodElapsedCallback(Chariot *chariot)
{
    Dart_FSM.Status[Dart_FSM.Now_Status_Serial].Time++;
    switch (Dart_FSM.Now_Status_Serial)
    {
    case 0:
        /*code*/
        Set_Dart_Parameter(chariot, ID, FRIC_CLOSE, push_stop,CAN_TX_STOP);
        if(Set_Yaw_Angle_1!=0 && Set_Yaw_Angle_2!=0)
            Set_Status(&Dart_FSM, 1);
        break;
    case 1:
        /*code*/
        Set_Dart_Parameter(chariot, ID, FRIC_CLOSE, push_stop,CAN_TX_OPEN); // 状态一：
        if (Last_Status == 1 && dart_client_cmd_t.dart_launch_opening_status == 2)
            Start_Flag = 1;
        if (Start_Flag == 1 &&
            dart_client_cmd_t.dart_launch_opening_status == Dart_State_Open)
            Set_Status(&Dart_FSM, 2);
        break;
    case 2:
        /*code*/
        Set_Dart_Parameter(chariot, ID, FRIC_OPEN, push_stop,CAN_TX_OPEN);               // 状态二：预设飞镖各个电机参数并驱动firc和yaw电机
        if (fabsf(chariot->dart_yaw.now_angle - chariot->dart_yaw.target_angle) < 0.005) // 判断yaw轴转到指定位置后开始推飞镖

        {
            if ((ID == 0 || ID == 2) &&
                (Dart_FSM.Status[Dart_FSM.Now_Status_Serial].Time >= 3000))
                Set_Status(&Dart_FSM, 3);
            else if ((ID == 1 || ID == 3) &&
                    (Dart_FSM.Status[Dart_FSM.Now_Status_Serial].Time >= 1000))
                Set_Status(&Dart_FSM, 3);
        }
        break;
    case 3:
        /*code*/
        Set_Dart_Parameter(chariot, ID, FRIC_OPEN, push_start,CAN_TX_OPEN); // 状态三：推飞镖
        if ((fabsf(chariot->dart_push.actual_length - Dart_parameter[ID][3]) < 0.001))
        {
            Set_Status(&Dart_FSM, 1);
            ID++;
            if (ID % 2 == 0)
                Start_Flag = 0;
        }
        break;
    }
}
void Task_Init(Chariot *chariot)
{
    Fric_Init(&chariot->dart_fric); // pid初始化
    Push_Init(&chariot->dart_push);
    Yaw_Init(&chariot->dart_yaw);
    Pitch_Init(&chariot->dart_pitch);
    Key_Init(&chariot->key);
}
/*************************************针对于每一发飞镖设置相应的参数 并 进行处理**************************************************************/
/**************************************function_函数********************************************88*/
void Set_Dart_Parameter(Chariot *chariot, uint8_t DART_ID, uint8_t FRIC_STATUS, uint8_t PUSH_CMD,uint8_t CAN_TX_CMD)
{
    chariot->can_tx_cmd = CAN_TX_CMD;
    // 摩擦轮预设外环内环速度和状态
    chariot->dart_fric.fric_status = (Fric_status)FRIC_STATUS;
    if (chariot->dart_fric.fric_status == FRIC_OPEN)
    {
        chariot->dart_fric.fric_motor[0].target_speed = (int16_t)Dart_parameter[DART_ID][1];
        chariot->dart_fric.fric_motor[1].target_speed = -(int16_t)Dart_parameter[DART_ID][2];
        chariot->dart_fric.fric_motor[2].target_speed = (int16_t)Dart_parameter[DART_ID][2];
        chariot->dart_fric.fric_motor[3].target_speed = -(int16_t)Dart_parameter[DART_ID][1];
    }
    else
    {
        for (int i = 0; i < 4; i++)
        {
            chariot->dart_fric.fric_motor[i].target_speed = 0;
        }
    }
    //  yaw和pitch预设角度
    chariot->dart_yaw.target_angle = Init_Angle + Dart_parameter[DART_ID][0];
    chariot->dart_pitch.target_length = Dart_parameter[DART_ID][4];
    //  push预设行程和状态
    chariot->dart_push.push_command = PUSH_CMD;
    chariot->dart_push.target_length = Dart_parameter[DART_ID][3];
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim2)
    {
        Dart_FSM_TIM_Status_PeriodElapsedCallback(&chariot);

        Yaw_Task(&chariot.dart_yaw);

        Pitch_Task(&chariot.dart_pitch);
        Fric_Task(&chariot.dart_fric);
        Push_Task(&chariot.dart_push);
        if(chariot.can_tx_cmd == CAN_TX_OPEN)
        {
            CAN_Send_Msg(&hcan1, CAN1_TX_Fric, CAN1_Fric_Tx_Data, 8);
            CAN_Send_Msg(&hcan2, CAN2_TX_Yaw, CAN2_Yaw_Tx_Data, 8);
            CAN_Send_Msg(&hcan2, CAN2_TX_Push_Pitch, CAN2_Push_Pitch_Tx_Data, 8);
        }
        
        Last_Status = dart_client_cmd_t.dart_launch_opening_status;
    }
    else if(htim == &htim3)
    {
        Key_Status_FSM(&chariot.key);
        if(chariot.key.key_event == set_yaw_angle_1)
            Set_Yaw_Angle_1 = chariot.dart_yaw.now_angle;
        else if(chariot.key.key_event == set_yaw_angle_2)
            Set_Yaw_Angle_2 = chariot.dart_yaw.now_angle;
    }
}
